#include "GeometricDefinitions.h"

namespace Putin{

Geometry::Geometry(AxisAlignedBoundingBox AABB, OrientedBoundingBox OBB,PhysicsObject* phyobj)
{
	mAABB = AABB;
	mOBB = OBB;
	mIsDesposed = false;
	mPhysicsObject = phyobj;
}

void Geometry::UpdateAABB(Vec3f position)
{
	mAABB.P = position;
	return;
}

void Geometry::UpdateOBB(Vec3f normal, Vec3f direction)
{
	mDirection = direction;
	/*
	 *	Code to update the OOB
	 */
	mOBB.P = mAABB.P;
	mOBB.Z = normal;
	/*
	 *  We wish to rotate mDirection and a 90 degree ofset copy to obtain
	 *  the rotated OOB. It is possible that this can be optimized either with
	 *	quaternions or by utilizing the length of the crossproduct and remove the tan2
	 */
	
	Vec3f r = (mOBB.Z^Vec3f(0,0,1)).normalized();	//Normalized rotational axis
	float a = -atan2(sqrt(mOBB.Z.y*mOBB.Z.y+mOBB.Z.x*mOBB.Z.x),mOBB.Z.z);

	float cos_a = cos(a);	//Is this good? or does the compiler optimize this automatically?
	float sin_a = sin(a);

	mOBB.X.x = (cos_a+(1-cos_a)*r.x*r.x)*mDirection.x;
	mOBB.X.y = ((1-cos_a)*r.x*r.y+r.z*sin_a)*mDirection.x;
	mOBB.X.z = ((1-cos_a)*r.x*r.z-r.y*sin_a)*mDirection.x;

	mOBB.X.x += ((1-cos_a)*r.x*r.y-r.z*sin_a)*mDirection.y;
	mOBB.X.y += (cos_a+(1-cos_a)*r.y*r.y)*mDirection.y;
	mOBB.X.z += ((1-cos_a)*r.y*r.z+r.x*sin_a)*mDirection.y;
	
	mOBB.Y = Vec3f(0,0,0)-(mOBB.X^mOBB.Z).normalized();
}

}//End namespace Putin

/*
*void CCCP::whatever(PhysicsBaseObject * obj)
{
	 if (typeid(*obj)==typeid( DynamicObject)
	 {
		DynamicObject* dynObj = (DynamicObject*)obj;
	 }
 }
*/